#include "board.h"
#include "bflb_mtimer.h"
#include "bflb_gpio.h"
#include "bflb_i2c.h"
#include "bflb_uart.h"

#include <stdio.h>
#include <math.h>

#include "OLED.h"
#include "MPU6050.h"
#include "MPU6050_Reg.h"
//-----------------------------------------------------------------------------------------------
/* Anotc */
#define BYTE0(dwTemp)       (*(char *)(&dwTemp))           //取出int型变量的低字节
#define BYTE1(dwTemp)       (*((char *)(&dwTemp) + 1))     //取存储在此变量下一内存字节的内容，高字节
//#define BYTE2(dwTemp)       (*((char *)(&dwTemp) + 2))
//#define BYTE3(dwTemp)       (*((char *)(&dwTemp) + 3))
//-----------------------------------------------------------------------------------------------
struct bflb_device_s *i2c1;
struct bflb_device_s *uartx;
//-----------------------------------------------------------------------------------------------
float MPU6050_AccelRange_div;                     //加速度敏感度
float MPU6050_GyroRange_div;                      //角速度敏感度
int16_t ax_16, ay_16, az_16;                      //读取16位x,y,z轴加速度
int16_t gx_16, gy_16, gz_16;                      //读取16位x,y,z轴陀螺仪角速度
float ax_f, ay_f, az_f;                           //经过敏感度换算后的浮点数x,y,z轴加速度
float gx_f, gy_f, gz_f;                           //经过敏感度换算后的浮点数x,y,z轴陀螺仪角速度
float pitch_raw, roll_raw, yaw_raw;               //初始俯仰角pitch,横滚角roll,偏航角yaw数据
float pitch_kalman, roll_kalman, yaw_kalman;      //滤波后数据
/* 卡尔曼滤波参数 */		
//static 
static float Q_angle = 0.03;	                  //角度数据置信度，角度噪声的协方差
static float Q_gyro  = 0.003;	              	  //角速度数据置信度，角速度噪声的协方差  
static float R_angle = 0.5;		                  //加速度计测量噪声的协方差
static float dt      = 0.0002;	                  //采样周期即计算任务周期
//-----------------------------------------------------------------------------------------------


void MPU6050_GPIO_Init(void)
{
    struct bflb_device_s *gpio;

    gpio = bflb_device_get_by_name("gpio");
    /* I2C_SDA */
    bflb_gpio_init(gpio, MPU6050_GPIO_SDA, MPU6050_FUNC_I2C | GPIO_ALTERNATE | GPIO_PULLUP | GPIO_SMT_EN | GPIO_DRV_1);
    /* I2C_SCL */
    bflb_gpio_init(gpio, MPU6050_GPIO_SCL, MPU6050_FUNC_I2C | GPIO_ALTERNATE | GPIO_PULLUP | GPIO_SMT_EN | GPIO_DRV_1);
}

void MPU6050_ShowFloatNum(uint8_t Line, uint8_t Column, float Float)
{
	char str[6]={0,0,0,0,0,0};
    
	// 将浮点数转换为字符串
	sprintf(str, "%3.2f", Float);

    // 在OLED上显示字符串
    OLED_ShowString(Line, Column, str);
}

void MPU6050_WriteReg(uint8_t I2CAddress,uint8_t RegAddress,uint8_t Byte)
{
    struct bflb_i2c_msg_s msgs[1];              //寻找i2c设备并发送缓存信息
    uint8_t message[2];                         //缓存信息，即i2c设备内寄存器的地址+一个字节
    message[0] = RegAddress;                    //i2c设备内寄存器的地址
    message[1] = Byte;                          //发送的一个字节

    msgs[0].addr = I2CAddress;
    msgs[0].flags = 0;
    msgs[0].buffer = message;
    msgs[0].length = 2;
    bflb_i2c_transfer(i2c1, msgs, 1);
}

uint8_t MPU6050_Multi_WriteReg(uint8_t I2CAddress,uint8_t RegAddress,uint8_t Lenth,uint8_t *Data)
{
    struct bflb_i2c_msg_s msgs[1];
    uint8_t message[Lenth + 1];
    uint8_t i;
    if(Lenth >= 1 && Data != NULL)
    {
        message[0] = RegAddress;
        for(i=1 ;i <= Lenth; i++)
        {
            message[i] = *(Data + i - 1);
        }
        msgs[0].addr = I2CAddress;
        msgs[0].flags = 0;
        msgs[0].buffer = message;
        msgs[0].length = Lenth + 1;
        bflb_i2c_transfer(i2c1, msgs, 1);
        return 0;
    }
    else
    {
        return 1;
    }
}

uint8_t MPU6050_ReadReg(uint8_t I2CAddress,uint8_t RegAddress)
{
    struct bflb_i2c_msg_s msgs[2];
    uint8_t message[1];
    uint8_t ReturnData[1];
    message[0] = RegAddress;
    msgs[0].addr = I2CAddress;
    msgs[0].flags = I2C_M_NOSTOP;
    msgs[0].buffer = message;
    msgs[0].length = 1;

	msgs[1].addr = I2CAddress;
    msgs[1].flags = I2C_M_READ;
    msgs[1].buffer = ReturnData;
    msgs[1].length = 1;
    bflb_i2c_transfer(i2c1, msgs, 2);
	return ReturnData[0];
}

uint8_t MPU6050_Multi_ReadReg(uint8_t I2CAddress,uint8_t RegAddress,uint8_t Lenth,uint8_t *Data)
{
    struct bflb_i2c_msg_s msgs[2];
    uint8_t message[1];
    if(Lenth >= 1)
    {
        message[0] = RegAddress;
        msgs[0].addr = I2CAddress;
        msgs[0].flags = I2C_M_NOSTOP;
        msgs[0].buffer = message;
        msgs[0].length = 1;

        msgs[1].addr = I2CAddress;
        msgs[1].flags = I2C_M_READ;
        msgs[1].buffer = Data;
        msgs[1].length = Lenth;
        bflb_i2c_transfer(i2c1, msgs, 2);
        return 0;
    }
    else
    {
        return 1;
    }
}

uint8_t MPU6050_AccelFullRange(uint8_t I2CAddress,uint8_t FullRange)
{
    uint8_t Register = 0x00;
    /*
    【ACCEL_CONFIG】
    默认不自检，全置0
    bit7 XA_ST=1 启动加速度计X轴的自检
    bit6 YA_ST=1 启动加速度计Y轴的自检
    bit5 ZA_ST=1 启动加速度计Z轴的自检
    
    bit[4:3] AFS_SEL 配置加速度计满量程
                    00为+-2g    MPU6050_AccelRange_2g
                    01为+-4g    MPU6050_AccelRange_4g
                    10为+-8g    MPU6050_AccelRange_8g
                    11为+-16g   MPU6050_AccelRange_16g

    bit[2:0] ACCEL_HPF 高通滤波器High Pass Filter
    */
    MPU6050_AccelRange_div  = 65536/(pow(2,(FullRange + 1))*2);
    Register |= (FullRange<<3);
    MPU6050_WriteReg(I2CAddress, MPU6050_ACCEL_CONFIG, Register);
    return 0;
}

uint8_t MPU6050_GyroFullRange(uint8_t I2CAddress,uint8_t FullRange)
{
    uint8_t Register = 0x00;
    /*
    【GYRO_CONFIG】
    默认不自检，全置0
    bit7 XG_ST=1 启动陀螺仪X轴的自检
    bit6 YG_ST=1 启动陀螺仪Y轴的自检
    bit5 ZG_ST=1 启动陀螺仪Z轴的自检
    
    bit[4:3] FS_SEL 配置陀螺仪满量程
                    00为+-250    MPU6050_GyroRange_250
                    01为+-500    MPU6050_GyroRange_500
                    10为+-1000   MPU6050_GyroRange_1000
                    11为+-2000   MPU6050_GyroRange_2000

    bit[2:0] 保留
    */
    MPU6050_GyroRange_div  = 65535/((pow(2,FullRange) * 250)*2);
    Register |= (FullRange<<3);
    MPU6050_WriteReg(I2CAddress, MPU6050_GYRO_CONFIG, Register);
    return 0;
}

uint8_t MPU6050_SetLPF(uint16_t I2CAddress,uint16_t LPF)
{
    uint8_t Register = 0;
    /*
    【CONFIG】
    bit[7:6] 保留
    bit[5:3] EXT_SYNC_SET 外部采用
    bit[2:0] DLPF_CFG 数字低通滤波器Digital Low Pass Filter
                      当DLPF_CFG=0/7时，陀螺仪输出频率为8KHz
                      当DLPF_CFG=1/2/3/4/5/6时，陀螺仪输出频率为1KHz
                      
    */
	if(LPF >= 188)        Register = 1;//Delay 1.9ms
	else if(LPF >= 98)    Register = 2;//Delay 2.8ms
	else if(LPF >= 42)    Register = 3;//Delay 4.8ms
	else if(LPF >= 20)    Register = 4;//Delay 8.3ms
	else if(LPF >= 10)    Register = 5;//Delay 13.4ms
	else                  Register = 6;//Delay 18.6ms
    MPU6050_WriteReg(I2CAddress, MPU6050_CONFIG, Register);
    return 0;
}

uint8_t MPU6050_SetRate(uint16_t I2CAddress,uint16_t Rate)
{
    uint8_t Register;
    /*
    【SMPLRT_DIV】基于陀螺仪输出速率的分频
    当DLPF_CFG=0或DLPF_CFG=7时，陀螺仪输出频率为8KHz
    其他情况时，陀螺仪输出频率为1KHz

    bit[7:0] SMPLRT_DIV 八位分配，最大255
    采样频率Rate = 陀螺仪输出频率(1KHz或8KHz) / (1 + SMPLRT_DIV)

    */
    if(Rate>1000)   Rate = 1000;
    if(Rate<4)      Rate = 4;
    Register = 1000 / Rate - 1;
    MPU6050_WriteReg(I2CAddress, MPU6050_SMPLRT_DIV, Register);
    MPU6050_SetLPF(I2CAddress, Rate/2);//自动设置LPF为采样率的一半
    return 0;
}

uint8_t MPU6050_Init(void)
{
    /* GPIO配置i2c初始化 */
    MPU6050_GPIO_Init();
    /* i2c1初始化 */
    i2c1 = bflb_device_get_by_name("i2c1");
    bflb_i2c_init(i2c1, 400000);
    /* MPU6050初始化 */
    MPU6050_WriteReg(MPU6050_ADDRESS, MPU6050_PWR_MGMT_1, 0x80);     //复位 进入睡眠模式
    bflb_mtimer_delay_ms(10);
    MPU6050_WriteReg(MPU6050_ADDRESS, MPU6050_PWR_MGMT_1, 0x00);     //唤醒

    MPU6050_AccelFullRange(MPU6050_ADDRESS, MPU6050_AccelRange_2g);  //配置加速度计满量程、自检 选择+-2g量程
    MPU6050_GyroFullRange(MPU6050_ADDRESS, MPU6050_GyroRange_2000);  //配置陀螺仪满量程、自检 选择+-2000量程

    MPU6050_SetRate(MPU6050_ADDRESS, 50);                            //配置陀螺仪采样频率为50Hz 且配置数字低通滤波
    
    MPU6050_WriteReg(MPU6050_ADDRESS, MPU6050_INT_ENABLE, 0x00);     //关闭中断
    MPU6050_WriteReg(MPU6050_ADDRESS, MPU6050_USER_CTRL, 0x00);      //关闭I2C主模式 不使能FIFO
    MPU6050_WriteReg(MPU6050_ADDRESS, MPU6050_FIFO_EN, 0x00);        //禁用FIFO
    MPU6050_WriteReg(MPU6050_ADDRESS, MPU6050_INT_PIN_CFG, 0x80);    //INT引脚低电平有效
    
    if(MPU6050_ReadReg(MPU6050_ADDRESS, MPU6050_WHO_AM_I) == MPU6050_ADDRESS)
    {
        MPU6050_WriteReg(MPU6050_ADDRESS, MPU6050_PWR_MGMT_1, 0x02);     //配置时钟源为 Y轴陀螺 PLL 
	    MPU6050_WriteReg(MPU6050_ADDRESS, MPU6050_PWR_MGMT_2, 0x00);     //不配置唤醒频率、待机模式
        MPU6050_SetRate(MPU6050_ADDRESS, 50);
    }
    else
        return 1;
    return 0;
}

uint8_t MPU6050_GetRawData_Accel(int16_t *ax, int16_t *ay, int16_t *az)
{
    uint8_t DataH, DataL;
	DataH = MPU6050_ReadReg(MPU6050_ADDRESS, MPU6050_ACCEL_XOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ADDRESS, MPU6050_ACCEL_XOUT_L);
    *ax = (DataH << 8) | DataL;

    DataH = MPU6050_ReadReg(MPU6050_ADDRESS, MPU6050_ACCEL_YOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ADDRESS, MPU6050_ACCEL_YOUT_L);
    *ay = (DataH << 8) | DataL;

    DataH = MPU6050_ReadReg(MPU6050_ADDRESS, MPU6050_ACCEL_ZOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ADDRESS, MPU6050_ACCEL_ZOUT_L);
    *az = (DataH << 8) | DataL;
    return 0;
}

uint8_t MPU6050_GetRawData_Gyro(int16_t *gx, int16_t *gy, int16_t *gz)
{
    uint8_t DataH, DataL;
	DataH = MPU6050_ReadReg(MPU6050_ADDRESS, MPU6050_GYRO_XOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ADDRESS, MPU6050_GYRO_XOUT_L);
    *gx = (DataH << 8) | DataL;

    DataH = MPU6050_ReadReg(MPU6050_ADDRESS, MPU6050_GYRO_YOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ADDRESS, MPU6050_GYRO_YOUT_L);
    *gy = (DataH << 8) | DataL;

    DataH = MPU6050_ReadReg(MPU6050_ADDRESS, MPU6050_GYRO_ZOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ADDRESS, MPU6050_GYRO_ZOUT_L);
    *gz = (DataH << 8) | DataL;
    return 0;
}

void Kalman_Cal_Pitch(float acc,float gyro) //卡尔曼滤波pitch轴计算		
{
	static float Q_bias;	//Q_bias:陀螺仪的偏差
	static float K_0, K_1;	//卡尔曼增益  K_0:用于计算最优估计值  K_1:用于计算最优估计值的偏差 t_0/1:中间变量
	static float PP[2][2] = { { 1, 0 },{ 0, 1 } };//过程协方差矩阵P，初始值为单位阵
	
	pitch_kalman += (gyro - Q_bias) * dt; //状态方程,角度值等于上次最优角度加角速度减零漂后积分
	
	PP[0][0] = PP[0][0] + Q_angle - (PP[0][1] + PP[1][0])*dt;
	PP[0][1] = PP[0][1] - PP[1][1]*dt;
	PP[1][0] = PP[1][0] - PP[1][1]*dt;
	PP[1][1] = PP[1][1] + Q_gyro;
	
	K_0 = PP[0][0] / (PP[0][0] + R_angle);
	K_1 = PP[1][0] / (PP[0][0] + R_angle);
	
	pitch_kalman = pitch_kalman + K_0 * (acc - pitch_kalman);

	Q_bias = Q_bias + K_1 * (acc - pitch_kalman);

	PP[0][0] = PP[0][0] - K_0 * PP[0][0];
	PP[0][1] = PP[0][1] - K_0 * PP[0][1];
	PP[1][0] = PP[1][0] - K_1 * PP[0][0];
	PP[1][1] = PP[1][1] - K_1 * PP[0][1];

    
}

void Kalman_Cal_Roll(float acc,float gyro)  //卡尔曼滤波roll轴计算				
{
	static float Q_bias;	//Q_bias:陀螺仪的偏差  Angle_err:角度偏量 
	static float K_0, K_1;	//卡尔曼增益  K_0:用于计算最优估计值  K_1:用于计算最优估计值的偏差 t_0/1:中间变量
	static float PP[2][2] = { { 1, 0 },{ 0, 1 } };//过程协方差矩阵P，初始值为单位阵
	roll_kalman += (gyro - Q_bias) * dt; //状态方程,角度值等于上次最优角度加角速度减零漂后积分
	PP[0][0] = PP[0][0] + Q_angle - (PP[0][1] + PP[1][0])*dt;
	PP[0][1] = PP[0][1] - PP[1][1]*dt;
	PP[1][0] = PP[1][0] - PP[1][1]*dt;
	PP[1][1] = PP[1][1] + Q_gyro;
	K_0 = PP[0][0] / (PP[0][0] + R_angle);
	K_1 = PP[1][0] / (PP[0][0] + R_angle);
	roll_kalman = roll_kalman + K_0 * (acc - roll_kalman);
	Q_bias = Q_bias + K_1 * (acc - roll_kalman);
	PP[0][0] = PP[0][0] - K_0 * PP[0][0];
	PP[0][1] = PP[0][1] - K_0 * PP[0][1];
	PP[1][0] = PP[1][0] - K_1 * PP[0][0];
	PP[1][1] = PP[1][1] - K_1 * PP[0][1];
}

void MPU6050_GetData(void)
{
    /* 获取MPU6050初始16位无符号数据 */
    MPU6050_GetRawData_Accel(&ax_16, &ay_16, &az_16);                       //获取加速度计初始数据
    MPU6050_GetRawData_Gyro(&gx_16, &gy_16, &gz_16);                        //获取陀螺仪初始数据
    /* 处理原始数据 */
    //加速度计取得的x,y,z轴16位数据经过敏感度换算
    if(ax_16<32764)   ax_f = ax_16/MPU6050_AccelRange_div;                  //计算x轴加速度
	else              ax_f = 1 - (ax_16-49152)/MPU6050_AccelRange_div;
    if(ay_16<32764)   ay_f = ay_16/MPU6050_AccelRange_div;                  //计算y轴加速度
	else              ay_f = 1 - (ay_16-49152)/MPU6050_AccelRange_div;
    if(az_16<32764)   az_f = az_16/MPU6050_AccelRange_div;                  //计算z轴加速度
	else              az_f = 1 - (az_16-49152)/MPU6050_AccelRange_div;
    //加速度反正切公式计算三个轴和水平面坐标系之间的夹角
    if(az_f == 0)
	{
		pitch_raw = 1;
		roll_raw = 1;
	}
	else
	{
		pitch_raw=(atan(ay_f/az_f))*180/3.14;
		roll_raw=(atan(ax_f/az_f))*180/3.14;
	}
    //判断计算后角度的正负号											
	if(ax_f<32764) roll_raw = +roll_raw;
	if(ax_f>32764) roll_raw = -roll_raw;
	if(ay_f<32764) pitch_raw = +pitch_raw;
	if(ay_f>32764) pitch_raw = -pitch_raw;
    //陀螺仪取得的x,y,z轴16位角速度数据经过敏感度换算
    if(gx_16<32768)   gx_f = -(gx_16/MPU6050_GyroRange_div);                //计算x轴角速度
	if(gx_16>32768)   gx_f = +(65535-gx_16)/MPU6050_GyroRange_div;
	if(gy_16<32768)   gy_f = -(gy_16/MPU6050_GyroRange_div);                //计算y轴角速度
	if(gy_16>32768)   gy_f = +(65535-gy_16)/MPU6050_GyroRange_div;
	if(gz_16<32768)   gz_f = -(gz_16/MPU6050_GyroRange_div);                //计算z轴角速度
	if(gz_16>32768)   gz_f = +(65535-gz_16)/MPU6050_GyroRange_div;
    /* 调用卡尔曼滤波 */
    //Kalman_Cal_Pitch(pitch_raw,gx_f);                                     //卡尔曼滤波计算X倾角
	Kalman_Cal_Roll(roll_raw,gy_f);                                       //卡尔曼滤波计算Y倾角
}
//*************************************************************************
void MPU6050_Anotc_UART_Init(void)
{
    struct bflb_uart_config_s cfg;
    cfg.baudrate = 2000000;
    cfg.data_bits = UART_DATA_BITS_8;
    cfg.stop_bits = UART_STOP_BITS_1;
    cfg.parity = UART_PARITY_NONE;
    cfg.flow_ctrl = 0;
    cfg.tx_fifo_threshold = 7;
    cfg.rx_fifo_threshold = 7;
    
    board_uartx_gpio_init();
    uartx = bflb_device_get_by_name("uart1");
    
    bflb_uart_init(uartx, &cfg);

}

void MPU6050_Anotc_SendFlightControl(void)
{
	uint8_t nul = 0;
	int16_t data = 0;
	uint8_t Data_to_Send[13] = {0};
	uint8_t _cnt = 0;
	uint8_t i;
	uint8_t sumcheck = 0;
	uint8_t addcheck = 0;
	
	Data_to_Send[_cnt++] = 0xAA;
	Data_to_Send[_cnt++] = 0xFF;
	Data_to_Send[_cnt++] = 0x03;
	Data_to_Send[_cnt++] = 7;
	data = -roll_kalman * 100;
	Data_to_Send[_cnt++] = BYTE0(data);
	Data_to_Send[_cnt++] = BYTE1(data);
	data= pitch_kalman * 100;
	Data_to_Send[_cnt++] = BYTE0(data);
	Data_to_Send[_cnt++] = BYTE1(data);
	data= yaw_kalman * 100;
	Data_to_Send[_cnt++] = BYTE0(data);
	Data_to_Send[_cnt++] = BYTE1(data);
	//FUSION _STA： 融合状态
	Data_to_Send[_cnt++] = BYTE0(nul);
	
	for(i = 0; i < _cnt; i++)
	{
		sumcheck += Data_to_Send[i];
		addcheck += sumcheck;
	}
	//if(sumcheck == Data_to_Send[Data_to_Send[3]+4] && addcheck == Data_to_Send[Data_to_Send[3]+5])
	Data_to_Send[_cnt++]=sumcheck;
	Data_to_Send[_cnt++]=addcheck;
    bflb_uart_put(uartx,Data_to_Send,_cnt);
}